"""
ArcBall.py -- Math utilities, vector, matrix types and ArcBall quaternion rotation class
Would not it be great to rotate your model at will, just by using the mouse?
With an ArcBall you can do just that.
This implementation of the ArcBall class based on Bretton Wade is, which is based on Ken Shoemake implementation
from the Graphic Gems series of books. However, with a little bug fixing and optimization for our purposes. 
"""
## The ArcBall works by mapping click coordinates in a window directly into the ArcBalls sphere coordinates,
## as if it were directly in front of you.

# Numeric is a discontinued extension to the Python programming language.
## It provides tools to manage large, multi-dimensional arrays and matrices with
## a large library of high-level mathematical functions. 
# The most recent, NumPy, is a merge between the two that builds on the code base
# of Numeric and adds the features of Numarray.
# http://en.wikipedia.org/wiki/NumPy
import numpy
## 'copy' module has a function named 'copy' that duplicate any object. 
# Copy a object is a alternative to the use of 'alias'.
import copy
from math import sqrt

# //assuming IEEE-754(GLfloat), which i believe has max precision of 7 bits
Epsilon = 1.0e-5

def sum (seq):
    try:
    	seq.__iter__()
    except:
    	return seq
    return reduce(lambda x, y: x + y, seq)

class ArcBallT:
# The coordinates were scaled down to the range of [-1...1] to make the math simpler;
# by coincidence this also lets the compiler do a little optimization.
## Next we calculate the length of the vector and determine whether or not it is
## inside or outside of the sphere bounds. If it is within the bounds of the sphere,
## we return a vector from within the inside the sphere, else we normalize the point
## and return the closest point to outside of the sphere. 
# Once we have both vectors, we can then calculate a vector perpendicular to the start
# and end vectors with an angle, which turns out to be a quaternion.
# With this in hand we have enough information to generate a rotation matrix from,
# and we are home free. 
        ## The ArcBall is instantiated using the following constructor. NewWidth and NewHeight
        ## are essentially the width and height of the window. 
	def __init__ (self, NewWidth, NewHeight):
		self.m_StVec = Vector3fT ()
		self.m_EnVec = Vector3fT ()
		self.m_AdjustWidth = 1.0
		self.m_AdjustHeight = 1.0
		self.setBounds (NewWidth, NewHeight)

	def __str__ (self):
		str_rep = ""
		str_rep += "StVec = " + str (self.m_StVec)
		str_rep += "\nEnVec = " + str (self.m_EnVec)
		str_rep += "\n scale coords %f %f" % (self.m_AdjustWidth, self.m_AdjustHeight)
		return str_rep

	def setBounds (self, NewWidth, NewHeight):
                ## If the window size changes, we simply update the ArcBall with that information: setBounds
 		# //Set new bounds
		assert (NewWidth > 1.0 and NewHeight > 1.0), "Invalid width or height for bounds."
		# //Set adjustment factor for width/height
		self.m_AdjustWidth = 1.0 / ((NewWidth - 1.0) * 0.5)
		self.m_AdjustHeight = 1.0 / ((NewHeight - 1.0) * 0.5)

        ## First we simply scale down the mouse coordinates from the range of [0...width), [0...height)
	## to [minus1...1], [1...minus1]  (Keep in mind that we flip the sign of Y so that we get the correct
	## results in OpenGL.) And this essentially looks like: 
	def _mapToSphere (self, NewPt):

		# Given a new window coordinate, will modify NewVec in place
		X = 0
		Y = 1
		Z = 2

		NewVec = Vector3fT ()
		# //Copy paramter into temp point
		TempPt = copy.copy (NewPt)
		# //Adjust point coords and scale down to range of [-1 ... 1]
		TempPt [X] = (NewPt [X] * self.m_AdjustWidth) - 1.0
		TempPt [Y] = 1.0 - (NewPt [Y] * self.m_AdjustHeight)
		# //Compute the square of the length of the vector to the point from the center
		length = sum(TempPt * TempPt)
                print "length = ",length
            		
		# //If the point is mapped outside of the sphere... (length > radius squared)
		if (length > 1.0):
			# //Compute a normalizing factor (radius / sqrt(length))
			norm    = 1.0 / sqrt (length);

			# //Return the "normalized" vector, a point on the sphere
			NewVec [X] = TempPt [X] * norm;
			NewVec [Y] = TempPt [Y] * norm;
			NewVec [Z] = 0.0;
		else:			# //Else it's on the inside
        	# //Return a vector to a point mapped inside the sphere sqrt(radius squared - length)
			NewVec [X] = TempPt [X]
			NewVec [Y] = TempPt [Y]
			NewVec [Z] = sqrt (1.0 - length)

		return NewVec

        def click (self, NewPt):
                ## When the user clicks the mouse, the start vector is calculated based on where the click occurred.    
		# //Mouse down (Point2fT
		self.m_StVec = self._mapToSphere (NewPt)
		return

        def drag (self, NewPt):
                ## When the user drags the mouse, the end vector is updated via the drag method, and if a quaternion
                ## output parameter is provided, this is updated with the resultant rotation. 
		# //Mouse drag, calculate rotation (Point2fT Quat4fT)
		""" drag (Point2fT mouse_coord) -> new_quaternion_rotation_vec
		"""
		X = 0
		Y = 1
		Z = 2
		W = 3

		self.m_EnVec = self._mapToSphere (NewPt)

		# //Compute the vector perpendicular to the begin and end vectors
		# Perp = Vector3fT ()
		Perp = Vector3fCross(self.m_StVec, self.m_EnVec);

		NewRot = Quat4fT ()
		# //Compute the length of the perpendicular vector
		if (Vector3fLength(Perp) > Epsilon):		#    //if its non-zero
			# //We're ok, so return the perpendicular vector as the transform after all
			NewRot[X] = Perp[X];
			NewRot[Y] = Perp[Y];
			NewRot[Z] = Perp[Z];
			# //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
			NewRot[W] = Vector3fDot(self.m_StVec, self.m_EnVec);
		else:		#                            //if its zero
			# //The begin and end vectors coincide, so return a quaternion of zero matrix (no rotation)
			NewRot[X] = NewRot[Y] = NewRot[Z] = NewRot[W] = 0.0;
			
		return NewRot


# ##################### Math utility ##########################################


def Matrix4fT ():
	return numpy.identity (4, 'f')

def Matrix3fT ():
	return numpy.identity (3, 'f')

def Quat4fT ():
	return numpy.zeros (4, 'f')

def Vector3fT ():
	return numpy.zeros (3, 'f')

def Point2fT (x = 0.0, y = 0.0):
	pt = numpy.zeros (2, 'f')
	pt [0] = x
	pt [1] = y
	return pt

def Vector3fDot(u, v):
	# Dot product of two 3f vectors
	dotprod = numpy.dot (u,v)
	return dotprod

def Vector3fCross(u, v):
	# Cross product of two 3f vectors
	X = 0
	Y = 1
	Z = 2
	cross = numpy.zeros (3, 'f')
	cross [X] = (u[Y] * v[Z]) - (u[Z] * v[Y])
	cross [Y] = (u[Z] * v[X]) - (u[X] * v[Z])
	cross [Z] = (u[X] * v[Y]) - (u[Y] * v[X])
	return cross

def Vector3fLength (u):
        # Teste 17-09
	mag_squared = sum(u * u)

	mag = sqrt (mag_squared)
	return mag
	
def Matrix3fSetIdentity ():
	return numpy.identity (3, 'f')

def Matrix3fMulMatrix3f (matrix_a, matrix_b):
        #return numpy.matrixmultiply (matrix_a, matrix_b)
        return numpy.dot(matrix_a, matrix_b)
	
def Matrix4fSVD (NewObj):
	X = 0
	Y = 1
	Z = 2
	s = sqrt ( 
		( (NewObj [X][X] * NewObj [X][X]) + (NewObj [X][Y] * NewObj [X][Y]) + (NewObj [X][Z] * NewObj [X][Z]) +
		(NewObj [Y][X] * NewObj [Y][X]) + (NewObj [Y][Y] * NewObj [Y][Y]) + (NewObj [Y][Z] * NewObj [Y][Z]) +
		(NewObj [Z][X] * NewObj [Z][X]) + (NewObj [Z][Y] * NewObj [Z][Y]) + (NewObj [Z][Z] * NewObj [Z][Z]) ) / 3.0 )
	return s

def Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix):
	# Modifies NewObj in-place by replacing its upper 3x3 portion from the 
	# passed in 3x3 matrix.
	# NewObj = Matrix4fT ()
	NewObj [0:3,0:3] = three_by_three_matrix
	return NewObj

# /**
# * Sets the rotational component (upper 3x3) of this matrix to the matrix
# * values in the T precision Matrix3d argument; the other elements of
# * this matrix are unchanged; a singular value decomposition is performed
# * on this object's upper 3x3 matrix to factor out the scale, then this
# * object's upper 3x3 matrix components are replaced by the passed rotation
# * components, and then the scale is reapplied to the rotational
# * components.
# * @param three_by_three_matrix T precision 3x3 matrix
# */
def Matrix4fSetRotationFromMatrix3f (NewObj, three_by_three_matrix):
	scale = Matrix4fSVD (NewObj)

	NewObj = Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix);
	scaled_NewObj = NewObj * scale			 # Matrix4fMulRotationScale(NewObj, scale);
	return scaled_NewObj

def Matrix3fSetRotationFromQuat4f (q1):
	# Converts the H quaternion q1 into a new equivalent 3x3 rotation matrix. 
	X = 0
	Y = 1
	Z = 2
	W = 3

	NewObj = Matrix3fT ()
	# Teste 17-09
	n = sum (q1 * q1)

	s = 0.0
	if (n > 0.0):
		s = 2.0 / n
	xs = q1 [X] * s;  ys = q1 [Y] * s;  zs = q1 [Z] * s
	wx = q1 [W] * xs; wy = q1 [W] * ys; wz = q1 [W] * zs
	xx = q1 [X] * xs; xy = q1 [X] * ys; xz = q1 [X] * zs
	yy = q1 [Y] * ys; yz = q1 [Y] * zs; zz = q1 [Z] * zs
	# This math all comes about by way of algebra, complex math, and trig identities.
	# See Lengyel pages 88-92
	NewObj [X][X] = 1.0 - (yy + zz);	NewObj [Y][X] = xy - wz; 			NewObj [Z][X] = xz + wy;
	NewObj [X][Y] =       xy + wz; 		NewObj [Y][Y] = 1.0 - (xx + zz);	NewObj [Z][Y] = yz - wx;
	NewObj [X][Z] =       xz - wy; 		NewObj [Y][Z] = yz + wx;          	NewObj [Z][Z] = 1.0 - (xx + yy)

	return NewObj






def unit_test_ArcBall_module ():
	# Unit testing of the ArcBall calss and the real math behind it.
	# Simulates a click and drag followed by another click and drag.
	print "unit testing ArcBall"
	Transform = Matrix4fT ()
	LastRot = Matrix3fT ()
	ThisRot = Matrix3fT ()

	ArcBall = ArcBallT (640, 480)
	# print "The ArcBall with NO click"
	# print ArcBall
	# First click
	LastRot = copy.copy (ThisRot)
	mouse_pt = Point2fT (500,250)
	ArcBall.click (mouse_pt)
	# print "The ArcBall with first click"
	# print ArcBall
	# First drag
	mouse_pt = Point2fT (475, 275)
	ThisQuat = ArcBall.drag (mouse_pt)
	# print "The ArcBall after first drag"
	# print ArcBall
	# print
	# print
	print "Quat for first drag"
	print ThisQuat
	ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat)
	# Linear Algebra matrix multiplication A = old, B = New : C = A * B
	ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot)
	Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot)
	print "First transform"
	print Transform
	# Done with first drag


	# second click
	LastRot = copy.copy (ThisRot)
	print "LastRot at end of first drag"
	print LastRot
	mouse_pt = Point2fT (350,260)
	ArcBall.click (mouse_pt)
	# second drag
	mouse_pt = Point2fT (450, 260)
	ThisQuat = ArcBall.drag (mouse_pt)
	# print "The ArcBall"
	# print ArcBall
	print "Quat for second drag"
	print ThisQuat
	ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat)
	ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot)
	# print ThisRot
	Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot)
	print "Second transform"
	print Transform
	# Done with second drag
	LastRot = copy.copy (ThisRot)

def _test ():
	# This will run doctest's unit testing capability.
	# see http://www.python.org/doc/current/lib/module-doctest.html
	#
	# doctest introspects the ArcBall module for all docstrings
	# that look like interactive python sessions and invokes
	# the same commands then and there as unit tests to compare
	# the output generated. Very nice for unit testing and
	# documentation.
	import doctest, ArcBall
	return doctest.testmod (ArcBall)

if __name__ == "__main__":
	# Invoke our function that runs python's doctest unit testing tool.
	_test ()

# unit_test ()
